#include "./app_task.h"
#include "../main_driver/cmd_uart.h"
#include "../pub_driver/cmd_can.h"
#include "../utils/log.h"
#include "../xlib/rcc.h"
#include "../main_driver/sensor.h"
#include "./handler_task.h"
#include "./status_task.h"
#include "../main_driver/motors.h"
#include "../main_driver/ctrl.h"

namespace main_task {

AppTask appTask;

using namespace main_driver;
using namespace pub_driver;
using namespace conn;

static void uart_event_loop();

template <typename RECEIVER>
struct CANConnTask : public rtos::Thread<CANConnTask<RECEIVER>, 1024, osPriorityHigh1> {

    CANConnTask(RECEIVER &recv) : m_recv(recv) {}

    void run() {
        Frame frame;
        for (;;) {
            m_recv.recv_from(frame);
            if (frame.dst() != addr::SELF) {
             //   println("send from can");
                cmdUart->write(frame.buf, frame.len);
                m_recv.recv_next(frame.len);
                continue;
            }
        }
    }

private:
    RECEIVER& m_recv;
};

static CANConnTask cargoConnTask(cmdCAN.cargoReceiver);
static CANConnTask heatConnTask(cmdCAN.heatReceiver);
static CANConnTask pearlConnTask(cmdCAN.pearlReceiver);
static CANConnTask iceConnTask(cmdCAN.iceReceiver);

void AppTask::run() {
    stm32f1::rcc::use_swd();
    cmdUart.init();
    cmdCAN.init();
    sensor.init();
    ctrl.init();
    motors.init();

    handlerTask.init_start();
    cargoConnTask.start();
    heatConnTask.start();
    pearlConnTask.start();
    iceConnTask.start();
    statusTask.start();

    println("main app run");

    uart_event_loop();
}

static void handle_fast_req(const Frame &frame) {

    cmdUart.ack(frame);
    cmdUart.recv_next(frame.len);
    handlerTask.post(frame);
}

static void handle_query(const Frame &frame) {
    switch (frame.seq_cmd()) {
        case proto::pub::QUERY_TASK_INFO: {
            const TaskInfo &taks_info = handlerTask.task_info;
            cmdUart.result(frame, taks_info.seq,taks_info.cmd);
        }; break;

    }
}

static void uart_event_loop() {

    Frame frame;

    for (;;) {
        cmdUart->recv_from(frame);
        
        if (frame.dst() != addr::SELF) {
            cmdCAN->write(frame.buf, frame.len);
            cmdUart->recv_next(frame.len);
            continue;
        }
        
        switch (frame.type()) {
            case type::PING: cmdUart.pong(frame); break;
            case type::FAST_REQ: handle_fast_req(frame); break;
            case type::QUERY: handle_query(frame); break;
        }
    }
}



}
